#include "Matrix4f.h"
#include "Vector.h"
#include <cstring>


Matrix4f::Matrix4f(){

    memset(m_data, 0, 16*sizeof(float));

    for(int i = 0; i<4; i++){
        m_data[i + 4*i] = 1.0f;
    }

}

Matrix4f::Matrix4f(float data[4][4])
{
    for(int i=0;i<4;i++)
    {
        for(int j=0;j<4;j++)
        {
            m_data[i + 4*j] = data[i][j];
        }
    }
}

float &Matrix4f::operator ()(int i, int j)
{
    return m_data[i + 4*j];
}

const float &Matrix4f::operator ()(int i, int j) const
{
    return m_data[i + 4*j];
}

Matrix4f Matrix4f::operator *(const Matrix4f &mat) const
{
    Matrix4f multMatrix;

    for(int i=0;i<4;i++)
    {
        for(int j=0;j<4;j++)
        {
            multMatrix(i,j) = 0;
            for(int k=0;k<4;k++)
            {
                multMatrix(i,j) = multMatrix(i,j) + (m_data[i + 4*k] * mat(k, j));
            }
        }
    }

    return multMatrix;
}

Vector3f Matrix4f::operator *(const Vector3f &vec) const
{
    float multMatrix[4];
    float resultMatrix[4] = {0,0,0,0};
    Vector3f resultVec;

    multMatrix[0] = vec.x;
    multMatrix[1] = vec.y;
    multMatrix[2] = vec.z;
    multMatrix[3] = 0;


    for(int i=0;i<4;i++)
    {
        for(int k=0;k<4;k++)
        {
            resultMatrix[i] = resultMatrix[i] + (m_data[i + 4*k] * multMatrix[k]);
        }
    }

    resultVec.x = resultMatrix[0];
    resultVec.y = resultMatrix[1];
    resultVec.z = resultMatrix[2];

    return resultVec;
}

Point3f Matrix4f::operator *(const Point3f &vec) const
{
    float multMatrix[4];
    float resultMatrix[4] = {0,0,0,0};
    Point3f resultPoint;

    multMatrix[0] = vec.x;
    multMatrix[1] = vec.y;
    multMatrix[2] = vec.z;
    multMatrix[3] = 1;

    for(int i=0;i<4;i++)
    {
        for(int k=0;k<4;k++)
        {
            resultMatrix[i] = resultMatrix[i] + (m_data[i + 4*k] * multMatrix[k]);
        }
    }

    resultPoint.x = resultMatrix[0];
    resultPoint.y = resultMatrix[1];
    resultPoint.z = resultMatrix[2];

    return resultPoint;
}

Matrix4f Matrix4f::transposed() const
{
    Matrix4f matrixTransp;

    for(int i=0;i<4;i++)
    {
        for(int k=0;k<4;k++)
        {
            matrixTransp(i,k) = m_data[4*i + k];
        }
    }

    return matrixTransp;
}
